//
// Created by cyq on 2022/12/5.
//

#include "task_chassis.h"
#include "driver_remote.h"

void ChassisTaskInit(void)
{
    uint8_t i;
    for (i = 1; i <= 4; i++) {
        ChassisMotorInit(&ChassisMotor[i]); //初始化底盘电机
    }
    PidInit(&ChassisMotor[4],8,0,0);
    SpeedLimitInit();
    chassisControlData.chassisMode = CHASSIS_MODE_FREE;
    PowerControlInit();
    //底盘跟随
    FollowControlInit(&followControlData);
}

void ChassisFollowControl(void)
{
    uint8_t i;
    for (i = 1; i <= 4; i++)
        ChassisMotor[i].PIDMode = MOTOR_PID_MODE_SPEED;

#if !USE_REMOTE_DATA
    ChassisFollowSetSpeed();
#endif
}
//小陀螺
void ChassisGyroControl(void)
{
    uint8_t i;
    for (i = 1; i <= 4; i++)
        ChassisMotor[i].PIDMode = MOTOR_PID_MODE_SPEED;
    ChassisGyroSetSpeed();
}

//吊射，自瞄等模式下，需要英雄的底盘进行一个自锁，不进行底盘的移动
void ChassisLockControl(void)
{
    chassisControlData.speedX = 0.f;
    chassisControlData.speedY = 0.f;
    chassisControlData.speedSpin = 0.f;
    chassisControlData.superFlag = 0;
    chassisControlData.disCharging = 0;

    //使用位置环完全锁住底盘，避免因为发射等活动进行移动
    for (uint8_t i = 1; i <= 4; i++)
    {
        ChassisMotor[i].PIDMode = MOTOR_PID_MODE_LOCATION;

        if (chassisControlData.lastMode != CHASSIS_MODE_LOCK)
            ChassisMotor[i].Location.SetLocation = ChassisMotor[i].Location.Location;
    }
}

void ChassisFreeControl(ChassisControlDataStruct* ChassisControlData)
{
    ChassisControlData->speedX = 0.f;
    ChassisControlData->speedY = 0.f;
    ChassisControlData->speedSpin = 0.f;
    ChassisControlData->superFlag = 0;
    ChassisControlData->disCharging = 0;
}

void ChassisControl(void)
{
    taskENTER_CRITICAL();
#if USE_REMOTE_DATA
    update_chassisControlData(&chassisControlData);
#endif
    switch (chassisControlData.chassisMode) {
        case CHASSIS_MODE_FOLLOW:
            ChassisFollowControl();
            ChassisMotorSetSpeed();
            break;
        case CHASSIS_MODE_GYRO:
            ChassisGyroControl();
            ChassisMotorSetSpeed();
            break;
        case CHASSIS_MODE_LOCK:
        case CHASSIS_MODE_AUTOSHOOT:
            ChassisLockControl();
            break;
        case CHASSIS_MODE_FREE:
            ChassisFreeControl(&chassisControlData);
            ChassisMotorSetSpeed();
            break;
    }
    chassisControlData.lastMode = chassisControlData.chassisMode;
    ChassisMotorCalculate();
    ChassisMotorDataSend();
    ChassisSpeedFeedback();
    taskEXIT_CRITICAL();
}

void ChassisTask(void *argument)
{
    /* USER CODE BEGIN ChassisTask */
    ChassisTaskInit();
    osDelay(200);
    /* Infinite loop */
    for(;;)
    {
        ChassisControl();
        osDelay(1);
    }
    /* USER CODE END ChassisTask */
}